/**************************************************************/
/*  This file is licensed from, and is a trade secret of:     */
/*                                                            */
/*    Mercury Computer Systems, Incorporated                  */
/*    199 Riverneck Road                                      */
/*    Chelmsford, Massachusetts 01824-2820                    */
/*    United States of America                                */
/*    Telephone + 1 978 256-1300                              */
/*                                                            */
/*  Refer to your Software License Agreement for provisions   */
/*  on use, duplication, or disclosure.                       */
/*  US. Government users and prime contractors are also       */
/*  governed by the following Data Rights Notice:             */
/*  Use, duplication or disclosure by the US. Government is   */
/*  subject to the provisions set forth in FAR 52.227-14 for  */
/*  restricted rights or other agency data rights provisions  */
/*  on commercial software and documentation, as applicable.  */
/*                                                            */
/*  This file: (a) was developed at private expense, and no   */
/*  part was developed with government funds, (b) is a        */
/*  trade secret of Mercury Computer Systems, Inc. for the    */
/*  purposes of the Freedom of Information Act, (c) is        */
/*  "commercial computer software" subject to limited         */
/*  utilization as provided in the above noted Software       */
/*  License Agreement and (d) is in all respects data         */
/*  belonging exclusively to Mercury Computer Systems, Inc..  */
/*                                                            */
/**************************************************************/

/******************************************************************/
/* Copyright (c) 1999-2003, Mercury Computer Systems, Inc.,       */
/* Chelmsford MA.,and all third party embedded software sources.  */
/* All rights reserved under the Copyright laws of US. and        */
/* international treaties.                                        */
/*                                                                */
/* Patent Pending                                                 */
/******************************************************************/


//$COPYRIGHT_END


#include <memory>
#include "msk.h"

using namespace std;

/*
 * Process in small chunks instead of big one or single samples.
 */

const int SampleBlock = 512;

extern "C" {
  MOD_EXPORT
  POA_CF::Resource *
  CreateMskModulator (const char * identifier)
  {
    MskModulator * Modulator;

    if (MskModulator::Create (Modulator, identifier) != success) {
      return 0;
    }

    return (POA_CF::Resource *) Modulator;
  }
}

status
MskModulator::Create (MskModulator *& handle,
                     const char * identifier)
{
  MskModulator * Temp = new MskModulator (identifier);

  if (!Temp) {
    handle = 0;
    return fail;
  }

  handle = Temp;
  return success;
}


MskModulator::MskModulator (const char * identifier)
  : CF__Resource_impl (identifier),
    m_pulse_shape (0),
    m_threadrunning (false),
    m_bStop (false)
{
  m_uiSamplesPerSymbol = 4; // must be at least 2 for MSK
  m_uiLenPulseShape = m_uiSamplesPerSymbol; 
  m_max_val = 32767;

  m_input_buffer_size = 1024;
  m_output_buffer_size = m_input_buffer_size*m_uiSamplesPerSymbol;

  // define ports
  m_fm3trin = new Fm3trInPort_impl (m_input_buffer_size);

  int m_output_buffer_size = m_uiSamplesPerSymbol * SampleBlock;
  m_output = new CMPLX_INT16 [m_output_buffer_size];
  m_complexout = new MskOutPort_impl (m_output, m_output_buffer_size);
}

MskModulator::~MskModulator ()
{
  delete m_fm3trin;
  delete m_complexout;
  delete [] m_output;
  delete [] m_pulse_shape;
}

CORBA::Object_ptr
MskModulator::getPort (const CORBA::Char * name)
  throw (CORBA::SystemException,
	 CF::PortSupplier::UnknownPort)
{
  string temp(name);

  if (temp == "MSK_ModulatorIn") {
    return m_fm3trin->getPort();
  }
  else if (temp == "MSK_ModulatorOut") {
    return m_complexout->getPort();
  }
  else {
    throw CF::PortSupplier::UnknownPort();
  }
}  //MskModulator::getPort()

void
MskModulator::start ()
  throw (CORBA::SystemException,
	 CF::Resource::StartError)
{
  if (m_threadrunning) {
    return;
  }

  m_threadrunning = true;
  m_thread.start (RunWorker, (void *) this);
  CF__Resource_impl::start();
} // MskModulator::start()


void
MskModulator::stop()
  throw (CORBA::SystemException,
	 CF::Resource::StopError)
{
  m_bStop = true;
  m_threadrunning = false;
  m_thread.detach();
} // MskModulator::stop()

/*****************************************************************************
 * Function: MskModulator::Init()
 *
 * Description: Initialize Modulator object. May be executed multiple 
 * times during the lifetime of an object to reinitialise object.
 *   
 * Operation:
 *   Inputs:
 *
 * Returns:
 *   success - creation successful
 *   fail - creation failed
 *
 * Notes/Issues:
 *****************************************************************************/

status
MskModulator::Init ()
{
#ifdef MCS_VERBOSE
  std::cout << "MskModulator: Initializing." << std::endl;
#endif

  // Generate pulse shape arrays.
  if (!m_pulse_shape) {
    m_pulse_shape = new short [m_uiLenPulseShape];
  }

  if (!m_pulse_shape) {
    return fail;
  }

  // Time constant.
  float t = static_cast<float>(PI/m_uiSamplesPerSymbol);

  /*
   * Pulse shape array; positive half of sine wave starting at first
   * sample after zero.
   */

  for (unsigned i=0; i<m_uiSamplesPerSymbol; i++) {
    m_pulse_shape[i] = (short) ((float)m_max_val*sin((float)(i+1)*t));
  }

  return success;
}

// Generic thread OS signature which calls component specific thread function
void
MskModulator::RunWorker (void * param)
{
  MskModulator * component = (MskModulator *) param;

#ifdef MCS_VERBOSE
  std::cout << "MskModulator: Component is running." << std::endl;
#endif

  component->RunLoop();

#ifdef MCS_VERBOSE
  std::cout << "MskModulator: Component is stopping." << std::endl;
#endif
}

// Inner loop function that checks for a stop command
void
MskModulator::RunLoop ()
{
  m_bStop = false;

  // initialize run conditions
  Init();

  while (!m_bStop) {
    Run ();
  }

  m_fm3trin->ReleaseDataBuffer ();
}

/*****************************************************************************
 * Function: status MskModulator::Modulate (
 *	char * bits,
 *	int numBits,
 *	CMPLX_INT16 * mod_syms,
 *	int & numSamples,
 *	char & last_Q_bit)
 *
 * Description: Generates a MSK modulated waveform at baseband given a vector
 *              of symbols.
 *   
 * Operation:
 *   Inputs:
 *	bits - modulation symbols (must be +-1)
 *	numBits - number of bits to modulate (must be even)
 *	last_Q_bit - one quadrature symbol overlaps sequence; need to know
 *		whether it was 0 or 1 from last time Modulate() was called
 *     
 *   Outputs:
 *	mod_syms - modulated symbols
 *	numSamples - number of complex samples in mod_syms
 *	last_Q_bit - value of last quadrature bit for continuity
 *
 *   Returns:
 *	status: success/fail
 *
 * Notes:
 *   1. *mod_syms must have a minimum length (numBits*m_uiSamplesPerSymbol/2)
 *   
 *  Prior to execution:
 *       *bits points to array of bits to encode (+1/-1)
 *	 numBits is even

 * Post execution
 *	 *mod_syms holds the MSK-modulated symbols
 *	 numSamples is the number of modulated saples
 *	 last_Q_bit holds the last quadrature bit (for continuity)
 *
 *****************************************************************************/

status
MskModulator::Modulate (char * bits,
			int numBits,
                        CMPLX_INT16 * mod_syms,
			int & numSamples,
                        char & last_Q_bit)
{

  if (numBits % 2) {
    // numBits must be even
    throw "MskModulator::Modulate(): numBits must be even";
  }

  CMPLX_INT16 * output_ptr_I = mod_syms; // in-phase write pointer
  CMPLX_INT16 * output_ptr_Q = mod_syms; // quadrature write pointer
  int sign_I;
  int sign_Q;
  unsigned half_pulse_length = m_uiLenPulseShape / 2;

  /*
   * Special condition for Q array: use only last half pulse shape for
   * continuity with previous array.
   */

  sign_Q = (last_Q_bit>0) ? 1 : -1;
  for (unsigned j=half_pulse_length; j<m_uiLenPulseShape; j++) {
    // Work around compiler issue.
#ifdef WIN32
    output_ptr_Q->imag (sign_Q * m_pulse_shape[j]);
#else
    short & image_ref = output_ptr_Q->imag();
    image_ref = sign_Q * m_pulse_shape[j];
#endif
    output_ptr_Q++;
  }

  for (int i=0; i<numBits; i+=2) {
    sign_I = (bits[i]>0)   ? 1 : -1;
    sign_Q = (bits[i+1]>0) ? 1 : -1;

    // Encode in-phase portion of waveform.
    for (unsigned j=0; j<m_uiLenPulseShape; j++) {
#ifdef WIN32
      output_ptr_I->real(sign_I * m_pulse_shape[j]);
#else
      short & image_ref = output_ptr_I->real ();
      image_ref = sign_I * m_pulse_shape[j];
#endif
      output_ptr_I++;
    }

    // Encode quadrature portion of waveform.
    if (i == (numBits-2)) {
      // Special condition for last symbol: use only first half of pulse.
      for (unsigned j=0; j<half_pulse_length; j++) {
	// Work around compiler issue.
#ifdef WIN32
	output_ptr_Q->imag (sign_Q * m_pulse_shape[j]);
#else
	short & image_ref = output_ptr_Q->imag ();
	image_ref = sign_Q * m_pulse_shape[j];
#endif
	output_ptr_Q++;
      }
      last_Q_bit = bits[i+1]; // Save for next round.
    }
    else {
      // Normal operation.
      for (unsigned j=0; j<m_uiLenPulseShape; j++) {
	// Work around compiler issue.
#ifdef WIN32
	output_ptr_Q->imag (sign_Q * m_pulse_shape[j]);
#else
	short & image_ref = output_ptr_Q->imag ();
	image_ref = sign_Q * m_pulse_shape[j];
#endif
	output_ptr_Q++;
      }
    } // if
  } // for

  /*
   * Verify that the same number of samples have been written to
   * both in-phase and quadrature components.
   */

  assert (output_ptr_I == output_ptr_Q);

  /*
   * Save and return the number of samples encoded (divide by 2 because
   * there are 2 bits per symbol with MSK).
   */

  numSamples = numBits * m_uiSamplesPerSymbol / 2;
  return success;
}

#ifdef MCS_VERBOSE
static int loop_counter=0;
#endif

// Inner loop method that performs component business logic.
status
MskModulator::Run ()
{
  int intotal=0, outtotal=0;

  /*
   * While the input stream is good and there are more symbols to process.
   */

  m_bStop = false;

  intotal = 0;
  outtotal = 0;

  int m_input_count = m_fm3trin->GetData (&m_input);

  if (m_input_count == 0) {
    return fail;
  }

#ifdef MCS_VERBOSE
  std::cout << "MskModulator: Received " << m_input_count
	    << " data samples." << std::endl;
#endif

  m_samples_read = m_input_count;

  // Cap number of samples read at a time to size of input buffer.
  m_samples_read = (m_samples_read > SampleBlock) ? SampleBlock : m_samples_read;

  // Use only even number of bits.
  m_samples_read -= (m_samples_read % 2) ? 1 : 0;

  if (m_samples_read > 0) {
    int num_mod_syms =  0;
    char last_Q_bit = 1;

    if (!Modulate (m_input, m_samples_read, m_output, num_mod_syms, last_Q_bit)) {
      m_fm3trin->ReleaseDataBuffer (m_samples_read);
      return fail;
    }

    m_complexout->pushData (m_output, num_mod_syms);
    m_fm3trin->ReleaseDataBuffer (m_samples_read);

#ifdef MCS_VERBOSE
    std::cout << "MskModulator: Modulating packet " << loop_counter++
	      << "; " << m_samples_read << "/" << num_mod_syms << " values."
	      << std::endl;
#endif

    intotal += m_samples_read;
    outtotal += num_mod_syms;
  } // if (m_samples_read > 0)

  return success;
}  // MskModulator::Run()

